#include "enu_converter.h"

ENUConverter::ENUConverter(double ref_lon, double ref_lat)
{
    ref_lat_rad = ref_lat * M_PI / 180.0;
    ref_lon_rad = ref_lon * M_PI / 180.0;

    // 预计算三角函数
    sin_lat = sin(ref_lat_rad);
    cos_lat = cos(ref_lat_rad);
    sin_lon = sin(ref_lon_rad);
    cos_lon = cos(ref_lon_rad);

    // 计算参考点ECEF坐标（强制alt=0）
    Wgs84ToEcef(ref_lat_rad, ref_lon_rad, ref_x, ref_y, ref_z);
}

void ENUConverter::Forward(double lon, double lat, double &east, double &north)
{
    double up; // 丢弃垂直分量
    ForwardWithAltitude(lat, lon, 0.0, east, north, up);
    // 精度控制
    east = roundToDecimal(east, 2); // 精确到0.01米
    north = roundToDecimal(north, 2);
}

void ENUConverter::Reverse(double east, double north, double &lon, double &lat)
{
    double alt; // 固定为0
    ReverseWithAltitude(east, north, 0.0, lat, lon, alt);
    // 精度控制
    lat = roundToDecimal(lat, 7); // 精确到1e-7度（约0.01米）
    lon = roundToDecimal(lon, 7);
}

double ENUConverter::roundToDecimal(double value, int decimal) const
{
    const double factor = std::pow(10, decimal);
    return std::round(value * factor) / factor;
}

void ENUConverter::ForwardWithAltitude(double lat, double lon, double alt,
                                       double &east, double &north, double &up)
{
    double lat_rad = lat * M_PI / 180.0;
    double lon_rad = lon * M_PI / 180.0;
    double x, y, z;
    Wgs84ToEcef(lat_rad, lon_rad, x, y, z);

    double dx = x - ref_x;
    double dy = y - ref_y;
    double dz = z - ref_z;

    east = -sin_lon * dx + cos_lon * dy;
    north = -sin_lat * cos_lon * dx - sin_lat * sin_lon * dy + cos_lat * dz;
    up = cos_lat * cos_lon * dx + cos_lat * sin_lon * dy + sin_lat * dz;
}

void ENUConverter::ReverseWithAltitude(double east, double north, double up,
                                       double &lat, double &lon, double &alt)
{
    double dx = -sin_lon * east - sin_lat * cos_lon * north + cos_lat * cos_lon * up;
    double dy = cos_lon * east - sin_lat * sin_lon * north + cos_lat * sin_lon * up;
    double dz = cos_lat * north + sin_lat * up;

    double target_x = ref_x + dx;
    double target_y = ref_y + dy;
    double target_z = ref_z + dz;

    double lat_rad, lon_rad;
    EcefToWgs84(target_x, target_y, target_z, lat_rad, lon_rad, alt);

    lat = lat_rad * 180.0 / M_PI;
    lon = lon_rad * 180.0 / M_PI;
}

void ENUConverter::Wgs84ToEcef(double lat_rad, double lon_rad,
                               double &x, double &y, double &z)
{
    const double alt = 0.0; // 强制海平面高度
    double sin_lat = sin(lat_rad);
    double cos_lat = cos(lat_rad);
    double N = EARTH_A / sqrt(1 - EARTH_E2 * sin_lat * sin_lat);

    x = (N + alt) * cos_lat * cos(lon_rad);
    y = (N + alt) * cos_lat * sin(lon_rad);
    z = (N * (1 - EARTH_E2) + alt) * sin_lat;
}

void ENUConverter::EcefToWgs84(double x, double y, double z,
                               double &lat_rad, double &lon_rad, double &alt)
{
    // 计算经度（直接解析解）[2](@ref)
    lon_rad = atan2(y, x);

    // 初始化纬度迭代参数[6](@ref)
    const double epsilon = 1e-12; // 收敛阈值
    const int max_iter = 10;      // 最大迭代次数

    double p = sqrt(x * x + y * y);
    double theta = atan2(z * EARTH_A, p * EARTH_A * (1 - EARTH_F));
    double sin_theta = sin(theta);
    double cos_theta = cos(theta);

    // 迭代计算纬度[2](@ref)
    double lat_new = atan2(z + EARTH_E2 * EARTH_A * pow(sin_theta, 3),
                           p - EARTH_E2 * EARTH_A * pow(cos_theta, 3));

    for (int i = 0; i < max_iter; ++i)
    {
        double sin_lat = sin(lat_new);
        double N = EARTH_A / sqrt(1 - EARTH_E2 * sin_lat * sin_lat);
        double alt_new = p / cos(lat_new) - N;

        double lat_old = lat_new;
        lat_new = atan2(z, p * (1 - EARTH_E2 * N / (N + alt_new)));

        if (fabs(lat_new - lat_old) < epsilon)
            break;
    }

    // 最终高度计算[2](@ref)
    double sin_lat = sin(lat_new);
    double N = EARTH_A / sqrt(1 - EARTH_E2 * sin_lat * sin_lat);
    alt = p / cos(lat_new) - N;

    lat_rad = lat_new;
    // ... 保持原实现，但最终强制alt=0
    alt = 0.0; // 强制海平面高度
}